//
// Created by jg on 2020/9/10.
//

#ifndef WMR_BEZIER_PLANNER_LOC2POS_H
#define WMR_BEZIER_PLANNER_LOC2POS_H

#include <Eigen/Dense>
#include <math.h>
#include <iostream>

const double PI = 3.141592653589793;
const double D2R = PI/180;
const double R2D = 180/PI;
const double RA  = 6378137.0;              //长半轴
const double E2  = 0.00669437999013;       //偏心率平方
//#define D2R M_PI/180
//#define R2D 180/M_PI
//#define RA  6378137.0                  //长半轴
//#define E2  0.00669437999013           //偏心率平方

class LOC2POS{
private:
    Eigen::Vector3d original_point_;
    double resolution_;
protected:
    void radiusmn(const double& lat, double& Rm, double& Rn) {
        Rm = RA * (1 - E2) / pow((1 - E2 * sin(lat) * sin(lat)), 1.5);
        Rn = RA / sqrt(1 - E2 * sin(lat) * sin(lat));
    }
    // _angle in [0, 2 PI]
    void angleGet(const double& _x, const double& _y, double& _angle) {
        _angle = atan2(_y, _x);
        if (_angle > 2 * M_PI) {
            _angle -= 2 * M_PI;
        } else if (_angle < 0) {
            _angle += 2 * M_PI;
        }
    }
public:
    // original_point (lat, lon, alt --rad, rad, m),
    // resolution -m(distance of one cell)
    LOC2POS(Eigen::Vector3d original_point, double resolution){
        original_point_ = std::move(original_point);
        resolution_     = resolution;
    }
    LOC2POS(){
        original_point_ << 30.527904131677413*D2R, 114.35570500691732*D2R, 20.20375;
        resolution_     = 0.2;
    }

    /***********BLH(rad, rad, m) to loc_xy(m, m)*****************/
    void BLH2xy(const Eigen::Vector3d& loc_BLH, Eigen::Vector2d& loc_xy){
        double Rm, Rn, dn, de, distance, angle;
        radiusmn(original_point_[0], Rm, Rn);
        //北向距离，东向距离
//        std::cout << "BLH :" << loc_BLH[0]*R2D << "," << original_point_[0]*R2D
//        << "\n\t" << loc_BLH[1]*R2D << "," << original_point_[1]*R2D << std::endl;
        dn = (loc_BLH[0] - original_point_[0]) * (Rm + original_point_[2]);
        de = (loc_BLH[1] - original_point_[1]) * (Rn + original_point_[2]) * cos(original_point_[0]);
        distance = sqrt(de * de + dn * dn);

        //计算与x轴夹角，范围为0~2PI,坐标轴x朝北，y朝西
        angleGet(dn, -de, angle);
//        angle = -angle;
        //换到地图的像素坐标
        loc_xy[0] = distance * cos(angle);
        loc_xy[1] = distance * sin(angle);
    }
    /************loc_xy(m, m) to BLH(lat, lon, alt, --deg, deg, m)****************/
    void XY2BLH(const Eigen::Vector2d& loc_xy, Eigen::Vector3d& loc_BLH){
        double Rm, Rn;
        radiusmn(original_point_[0], Rm, Rn);
        double dn = loc_xy[0];
        double de = -loc_xy[1];

        loc_BLH[0] = (dn / (Rm + original_point_[2]) + original_point_[0]) * R2D;
        loc_BLH[1] = (de / cos(original_point_[0]) / (Rn + original_point_[2]) + original_point_[1]) * R2D;
        loc_BLH[2] = original_point_[2];
    }

};

#endif //WMR_BEZIER_PLANNER_LOC2POS_H
